#include "VsomeipClient.h"
#include <vsomeip/vsomeip.hpp>
#include <vsomeip/enumeration_types.hpp>

VsomeipClient::VsomeipClient()
    : app_(vsomeip::runtime::get()->create_application("adas_someip")), itsThread()
{
}

VsomeipClient& VsomeipClient::instance()
{
    static VsomeipClient sev;
    return sev;
}

void VsomeipClient::startup()
{
    init();
    start();
}

void VsomeipClient::teardown()
{
    stop();
}

bool VsomeipClient::init()
{
    if (!app_->init()) {
        std::cerr << "Couldn't initialize application" << std::endl;
        return false;
    }

    app_->register_state_handler( std::bind(&VsomeipClient::on_state, this, std::placeholders::_1));

    app_->register_message_handler(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_H264_IMG_EVENT_ID, std::bind(&VsomeipClient::on_image, this, std::placeholders::_1));
    app_->register_message_handler(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_DETECT_BBOX, std::bind(&VsomeipClient::on_face_detect, this, std::placeholders::_1));
    app_->register_message_handler(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_KEYPOINT, std::bind(&VsomeipClient::on_dms_keypoint, this, std::placeholders::_1));

    app_->register_availability_handler(DMS_SERVICE_ID, DMS_INSTANCE_ID, std::bind(&VsomeipClient::on_availability, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

    std::set<vsomeip::eventgroup_t> evGroup;
    evGroup.insert(DMS_EVENTGROUP_ID);
    app_->request_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_H264_IMG_EVENT_ID, evGroup, vsomeip_v3::event_type_e::ET_EVENT, vsomeip_v3::reliability_type_e::RT_UNKNOWN);
    app_->request_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_DETECT_BBOX, evGroup, vsomeip_v3::event_type_e::ET_EVENT, vsomeip_v3::reliability_type_e::RT_UNKNOWN);
    app_->request_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_KEYPOINT, evGroup, vsomeip_v3::event_type_e::ET_EVENT, vsomeip_v3::reliability_type_e::RT_UNKNOWN);
    app_->subscribe(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_EVENTGROUP_ID);

    return true;
}

void VsomeipClient::start()
{
    itsThread = std::thread([this](){
        app_->start();
    });
}

void VsomeipClient::stop()
{
    app_->clear_all_handler();

    app_->unsubscribe(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_EVENTGROUP_ID);
    app_->release_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_H264_IMG_EVENT_ID);
    app_->release_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_DETECT_BBOX);
    app_->release_event(DMS_SERVICE_ID, DMS_INSTANCE_ID, DMS_KEYPOINT);
    app_->release_service(DMS_SERVICE_ID, DMS_INSTANCE_ID);
    app_->stop();

    if(itsThread.joinable())
    {
        itsThread.join();
    }
}

void VsomeipClient::on_state(vsomeip::state_type_e _state)
{
    if (_state == vsomeip::state_type_e::ST_REGISTERED)
    {
        app_->request_service(DMS_SERVICE_ID, DMS_INSTANCE_ID);
    }
}

void VsomeipClient::on_availability(vsomeip::service_t _service, vsomeip::instance_t _instance, bool _is_available)
{
    std::cout << "Service ["
              << std::setw(4) << std::setfill('0') << std::hex << _service << "." << _instance
              << "] is "
              << (_is_available ? "available." : "NOT available.")
              << std::endl;
}

void VsomeipClient::on_image(const std::shared_ptr<vsomeip::message>& event)
{
    printEvent(event);
}

void VsomeipClient::on_face_detect(const std::shared_ptr<vsomeip::message>& event)
{
    printEvent(event);
}

void VsomeipClient::on_dms_keypoint(const std::shared_ptr<vsomeip::message>& event)
{
    printEvent(event);
}

void VsomeipClient::printEvent(const std::shared_ptr<vsomeip::message>& event)
{
    std::stringstream its_message;
    its_message << "Received an Event ["
                << std::setw(4)    << std::setfill('0') << std::hex
                << event->get_service() << "."
                << std::setw(4) << std::setfill('0') << std::hex
                << event->get_instance() << "."
                << std::setw(4) << std::setfill('0') << std::hex
                << event->get_method() << "] to Client/Session ["
                << std::setw(4) << std::setfill('0') << std::hex
                << event->get_client() << "/"
                << std::setw(4) << std::setfill('0') << std::hex
                << event->get_session()
                << "]";
    // Dump payload: if it looks like printable ASCII (e.g. NMEA sentence), print as string,
    // otherwise fall back to a hex byte dump.
    auto payload = event->get_payload();
    if (payload) {
        auto len = payload->get_length();
        const auto *data = payload->get_data();

        bool printable = true;
        for (vsomeip::length_t i = 0; i < len; ++i) {
            unsigned char c = static_cast<unsigned char>(data[i]);
            // allow common control chars used in text protocols (CR, LF, TAB)
            if (c < 0x20 && c != '\r' && c != '\n' && c != '\t') { printable = false; break; }
            if (c == 0x7f) { printable = false; break; }
        }

        if (printable) {
            // Build a std::string from payload (preserve content including CR/LF)
            std::string s(reinterpret_cast<const char*>(data), static_cast<size_t>(len));
            // Trim trailing nulls (if any) for nicer printing
            while (!s.empty() && s.back() == '\0') s.pop_back();
            its_message << " Payload(as-string, len=" << len << "): " << s;
        } else {
            std::ostringstream hex;
            for (vsomeip::length_t i = 0; i < len; ++i) {
                hex << std::setw(2) << std::setfill('0') << std::hex << int(static_cast<unsigned char>(data[i])) << ' ';
            }
            its_message << " Payload(" << len << "): " << hex.str();
        }
    }
    std::cout << its_message.str() << std::endl;
}
